#!/usr/bin/env python

import roslib; roslib.load_manifest('bwi_apps')
import rospy, actionlib

from bwi_msgs.msg import PersonDetection
from bwi_msgs.msg import PersonDetectionArray
from bwi_msgs.msg import RobotDetection
from bwi_msgs.msg import RobotDetectionArray
from bwi_msgs.msg import MultiLevelMapPoint
from bwi_msgs.srv import NavigatePerson
from bwi_msgs.srv import MakeNavPlan
from bwi_msgs.srv import NavigatePersonResponse
from move_base_msgs.msg import MoveBaseAction
from move_base_msgs.msg import MoveBaseGoal
from geometry_msgs.msg import PoseStamped
import bwi_utils.utils
utils = bwi_utils.utils

import math
import time

class BWINavigator:
  def __init__(self):
    rospy.init_node('bwi_navigator')

    #get parameters
    try:
      self.robot_list = rospy.get_param('~robot_list')
    except:
      rospy.logfatal("Please provide robot list (~robot_list)")
      return

    self.robot_controller = dict()
    #robot controllers
    for level, robot in self.robot_list.iteritems():
      rospy.loginfo("Waiting for robot: %s " %(robot));
      self.robot_controller[robot] = actionlib.SimpleActionClient("/" + robot + "/move_base", MoveBaseAction)
      self.robot_controller[robot].wait_for_server()

    rospy.loginfo("Waiting for path provider");
    rospy.wait_for_service('/path_provider/make_plan')
    self.makeNavPlan = rospy.ServiceProxy('/path_provider/make_plan', MakeNavPlan)

    self.current_persons = dict()
    self.current_robots = dict()

    self.person_subscriber = rospy.Subscriber("/global/person_detections", PersonDetectionArray, self.personCallback)
    self.robot_subscriber = rospy.Subscriber("/global/robot_detections", RobotDetectionArray, self.robotCallback)
    self.service_server = rospy.Service("/navigate", NavigatePerson, self.navigateCallback) 

    rospy.loginfo("Starting spin");
    rospy.spin()

  def personCallback(self, data):
    for person in data.detections:
      if person.id:
        self.current_persons[person.id] = person

  def robotCallback(self, data):
    for robot in data.detections:
      self.current_robots[robot.id] = robot

  def getPoseFromPerson(self, person):
    return self.getPoseFromPoint(person.feet, utils.frameIdFromLevelId(person.level_id))

  def getPoseFromPoint(self, point, frame):
    pose = PoseStamped()
    pose.header.frame_id = frame
    pose.header.stamp = rospy.Time.now()
    pose.pose.position = point
    #some better way to calculate orientation?
    pose.pose.orientation.x = 0
    pose.pose.orientation.y = 0
    pose.pose.orientation.z = 0
    pose.pose.orientation.w = 1
    return pose

  def waitForPerson(self, person, point, map):
    maxDistance = 1.0
    timeout = 30
    start = time.time()
    while True:
      if person.id in self.current_persons:
        person = self.current_persons[person.id]
        distance = math.sqrt((point.x - person.feet.x) ** 2 + (point.y - person.feet.y) ** 2)
        if distance < maxDistance:
          return True
      time.sleep(0.1)
      if time.time() - start > timeout:
        break
    rospy.loginfo("waited too long for the person")
    return False

  def navigateCallback(self, data):
    response = NavigatePersonResponse()

    person_id = data.person_id
    if not person_id in self.current_persons: #we don't know where this person is
      response.success = False
      response.message("person %i not known to the navigator" % person_id)
      return response
    person = self.current_persons[person_id]
    level1 = person.level_id
    level2 = data.goal.level_id
    frame_id1 = utils.frameIdFromLevelId(level1)
    frame_id2 = utils.frameIdFromLevelId(level2)
    if level1 not in self.robot_list or level2 not in self.robot_list:
      response.success = False
      response.message = "invalid person/goal levels supplied to the navigator"
      return response
    robot_controller1 = self.robot_controller[self.robot_list[level1]]
    robot_controller2 = self.robot_controller[self.robot_list[level2]]
    rospy.loginfo("navigating person %i from %s to %s" % (person_id, level1, level2))

    #send robot1 to person
    goal1 = MoveBaseGoal()
    goal2 = MoveBaseGoal()
    goal1.target_pose = self.getPoseFromPerson(person)
    robot_controller1.send_goal(goal1)
    robot_controller1.wait_for_result()

    # see if destination is on the same map or not
    if level1 == level2:
      goal1.target_pose = self.getPoseFromPoint(data.goal.point, frame_id1)
      robot_controller1.send_goal(goal1)
      rospy.loginfo("robot approaching the person")
      robot_controller1.wait_for_result()
      response.success = True
      return response

    # otherwise find grand master navigation plan and move both robots into position
    rospy.loginfo("approach complete, waiting for navigation plan")
    start = MultiLevelMapPoint()
    start.level_id = level1
    start.point = person.feet
    plan = self.makeNavPlan(start, data.goal)
    if not plan.plan_found:
      rospy.logerr("plan request failed: %s" % plan.error_message)
      response.success = False
      return response

    #otherwise move robots
    goal1.target_pose = self.getPoseFromPoint(plan.start_level_goal, frame_id1)
    robot_controller1.send_goal(goal1)
    goal2.target_pose = self.getPoseFromPoint(plan.goal_level_start, frame_id2)
    robot_controller2.send_goal(goal2)

    rospy.loginfo("navigation plan obtained and assigned, waiting for completion")
    #wait for both robots to complete
    robot_controller1.wait_for_result()
    robot_controller2.wait_for_result()

    rospy.loginfo("robot goals complete, waiting for the person")
    if not self.waitForPerson(person, plan.goal_level_start, level2):
      response.success = False
      return response

    rospy.loginfo("person arrived, navigating to final destination")
    #finally, move robot2 to final goal
    goal2.target_pose = self.getPoseFromPoint(data.goal.point, frame_id2)
    robot_controller2.send_goal(goal2)
    robot_controller2.wait_for_result()
    rospy.loginfo("navigation complete")

    response.success = True
    return response



if __name__ == '__main__':
  BWINavigator()
